Reduced state estimation with multisensor fusion and out-of-sequence measurements

ABSTRACT

This invention relates to state estimation after processing measurements with time delays from multiple sensors of systems characterized by state variables and by multidimensional parameters, for which the latter are unknown and may vary arbitrarily in time within known physical bounds. If a measurement is time-late, apply the measurement to an out-of-sequence filter that uses a mean square optimization criterion that accounts for measurement errors and said bounding values, as well as the delay time, to optimally produce estimates of the true states of the system. If the measurement is not time-late, apply the measurements to an in-sequence filter that uses a mean square optimization criterion that accounts for measurement errors and said bounding values, to produce estimates of the true states of the system. The estimates are applied to one of (a) making a decision relating to the system, (b) operating a control system, and (c) controlling a process.

FIELD OF THE INVENTION

This invention relates generally to state estimation after processingmeasurements with time delays from multiple sensors of systemscharacterized by state variables and by multidimensional parameters, forwhich the latter are unknown and may vary arbitrarily in time withinknown physical bounds. In a particular aspect, the invention relates tothe tracking of moving targets using estimation, which takes intoconsideration delays in reporting measurements from multiple sources,measurement errors, and physical bounds or limits on parameters of thetarget track.

BACKGROUND OF THE INVENTION

FIG. 1 represents a simplified target tracking system 10. System 10tracks a target, illustrated as being an aircraft 12, by the use ofmultiple radar systems 14, 16. Radar system 14 includes a radar antenna14 a, which transmits and receives radar signals illustrated by“lightning bolt” symbols 18. Portions of the transmitted signals 18 arereflected by target 12 and return to the radar antenna 14 a. Thereturned signals allow the generation of measurements at an output port14 o of radar system 14. Radar system 16 includes a radar antenna 16 a,which transmits and receives radar signals illustrated by “lightningbolt” symbols 20. Portions of the transmitted signals 20 are reflectedby target 12 and return to the radar antenna 16 a. The returned signalsallow the generation of measurements at an output port 16 o of radarsystem 16. These measurements include values of at least targetposition, possibly in the form of range and angles from the radarsystems 14 and 16. A possible scenario is that radar system 14 has lessaccuracy (higher measurement noise) but provides more current datawhereas radar system 16 is more accurate (less measurement noise) butprovides data with time latency. The measurements are applied to aprocessing arrangement 22, which determines from the measurementsvarious target parameters, which may include course (direction ofmotion), speed, and target type. The estimated position of the target,and possibly other information, is provided to a utilization apparatusor user, illustrated in this case as being a radar display 24. Theoperator (or possibly automated decision-making equipment) can makedecisions as to actions to be taken in response to the displayedinformation. It should be understood that the radar tracking system 10of FIG. 1 is only one embodiment of a general class of estimationsystems for systems with distributed sensors such as nuclear, chemical,or manufacturing factories or facilities, control processes subject toexternal parameter changes, space station subject to vibrations,automobile subject to weather conditions, and the like.

State-of-the-art tracking systems utilize measurements fed to aprocessing site from multiple sensors. These sensors may have differentmeasuring accuracies and may be geographically dispersed over a regionof interest. Availability of reliable high bandwidth communication mediaallows such a topology of distributed multiple sensors for real-timeprocessing of the measurements.

In spite of today's high bandwidth and fast switching communicationnetwork, physical distances, path diversity and relays may result indifferent delays from various sensors to the processing site. Let asensor S₁ measure a tracked object at time t₁ and a sensor S₂ measurethat same object at time t₂ where t₂>t₁. It is possible that themeasurement from sensor S₁ may arrive many sampling intervals after themeasurement from sensor S₂ has already been processed. A simple decisionmethodology is to throw out the late-arriving measurement from sensorS₁, and not process it at all. However, if sensor S₁ is the moreaccurate sensor, this methodology does not make good use of that sensor.

A difficulty is that accounting for measurements received out ofsequence, as frequently happens in situations of multiple sensortracking with variable communication delays between sensors, greatlycomplicates the design of a Kalman filter, particularly when more thanone subsequent measurement is processed before an out-of-sequencemeasurement is received as indicated in Y. Bar-Shalom, “Update withOut-of-Sequence Measurements in Tracking: Exact Solution,” IEEETransactions on Aerospace and Electronic Systems, pp. 769–778, Vol.AES-38, No. 3, July 2002, J. R. Moore and W. D. Blair, “PracticalAspects of Multisensor Tracking,” in Multitarget-Multisensor Tracking:Applications and Advances, Volume III, Y. Bar-Shalom and William DaleBlair, (ed.), Boston, Mass.: Artech House, 2000, pp. 43–44, andPortmann, Moore, and Bath. Currently, even with rapid communications,delays of up to one second are not uncommon. As described below, evensuch small delays may have a significant effect on multisensor fusiontracking performance. Unlike smoothing and filtering, “how to update thecurrent state estimate with an “older” measurement is a nonstandardestimation problem” as quoted from Y. Bar-Shalom, M. Mallick, H. Chen,and R. Washburn, “One-Step Solution for the GeneralOut-of-Sequence-Measurement Problem in Tracking,” Proceedings of 2002IEEE Aerospace Conference Proceedings, Volume 4, pp. 1551–1559, March2002. No one definitive approach has yet been developed for this. Theabove is the opinion of Y. Bar-Shalom; Y. Bar-Shalom, M. Mallick, H.Chen, and R. Washburn; S. Challa and J. A. Legg, “Track-to-Track Fusionof Out-of-Sequence Tracks,” Proceedings of the Fifth InternationalConference on Information Fusion, pp. 919–926, July 2002; S. Challa, R.J. Evans, X. Wang, and J. Legg, “A Fixed-Lag Smoothing Solution toOut-of-Sequence Information Fusion Problems,” Communications inInformation and Systems, pp. 325–348, Vol. 2, No. 4, December 2002; M.L. Hernandez, A. D. Marrs, S. Maskell, and M. R. Orton, “Tracking andfusion for wireless sensor networks,” Proceedings of the FifthInternational Conference on Information Fusion, Vol. 2, pp. 1023–1029,July 2002; M. Ito, S. Tsujimichi, and Y. Kosuge, “Target Tracking withTime-Delayed Data in Multiple Radar System,” Proceedings of the 37^(th)SICE Annual Conference, pp. 939–944, July 1998; J. R. Moore and W. D.Blair; M. Mallick, S. Coraluppi, and C. Carthel, “Advances inAsynchronous and Decentralized Estimation,” Proceedings of 2001 IEEEAerospace Conference Proceedings, Vol. 4, pp. 1873–1888, March 2001; M.Mallick, J. Krant, and Y. Bar-Shalom, “Multi-sensor Multi-targetTracking using Out-of-sequence Measurements,” Proceedings of the FifthInternational Conference on Information Fusion, Vol. 1, pp. 135–142,July 2002. This is particularly true when it is desired that duringtarget maneuvers, the state estimate at the time of the current update(as opposed to at the past time of the “older” measurement, as insmoothing) have minimal covariance.

Filters without plant noise can optimally process out-of-sequencemeasurements in the order that they are received as stated by G. J.Portmann, J. R. Moore, and W. G. Bath supra. An optimal reduced stateestimator has been developed that approximates the higher derivatives oftarget motion with constant parameters belonging to a multivariateGaussian distribution as in the patent application entitled “REDUCEDSTATE ESTIMATOR FOR SYSTEMS WITH PHYSICALLY BOUNDED PARAMETERS,” filedMar. 16, 2005, in the names of P. Mookerjee and F. Reifler. Thisestimator does not need the white plant noise required by Kalman filterto cope with the reduced state. Among all estimators (including reducedstate Kalman filters) with the same reduced states, the optimal reducedstate estimator has minimal covariance. This covariance is the minimalcovariance achievable by linearly weighting the predicted states with anew measurement at each successive update of the filter. Since parameteruncertainty is included in the total error covariance that is minimized,the optimal reduced state estimator does not need white plant noise tocope with the reduced state.

Algorithms in the prior art are based on the Kalman filter. Thesealgorithms either set the white process noise to zero while processingan out-of-sequence measurement (which does not achieve goodperformance), or using a non-zero white process noise, provide solutionsfor processing measurements which are late by at most a few updateintervals. No algorithm exists in the prior art for providing an optimalsolution when delays occur that are longer than a few update intervals.It is common, however, that delays may be large such as ten or moreupdate intervals, which the current algorithms do not address.

In the prior art, the solutions are approximate as stated in P. J.Lanzkron and Y. Bar-Shalom, “A Two-Step Method for Out-Of-SequenceMeasurements,” Proceedings of the IEEE Aerospace Conference, Big Sky,Mont., March 2004, pp. 2036–2041. These approximations are not availablefor delays larger than a few update intervals.

Improved or alternative estimators are desired for coping without-of-sequence measurements that are late by a number of updateintervals.

SUMMARY OF THE INVENTION

In general, the invention relates to state estimation derived frommeasurements from multiple sensors with time delays. The invention isparticularly applicable to state estimation when some measurementsarrive late by large number of update intervals, which may be as much asten, or even more intervals late.

A method according to an aspect of the invention is for recursivelyestimating the state of a system having multidimensional parameters inaddition to state variables, which parameters are unknown, arbitrarilytime-varying, except for known bounded values. For example, the turnrate and tangential acceleration of an aircraft are multidimensionalarbitrarily time-varying parameters that have known bounds, in additionto the state of the aircraft given by its position and velocity. Saidstate estimates are derived from measurements subject to time delays andmeasurement errors. The state estimates are used to make decisions or tooperate a control system or to control a process.

A method according to another aspect of the invention is for estimatingthe state of a system comprising the steps of observing a system havingstate variables and also having unknown, multidimensional, arbitrarilytime-varying parameters, but which are subject to known bounded valuesand driven by a time-varying input function that depends on the states,and multidimensional parameters, measuring certain aspects of the stateof the system in the presence of measurement errors to produce initialmeasurements, initializing state estimates and matrices using a prioriinformation and the initial measurements. Then, the update interval isused in determining the system transition matrices and the mean value ofunknown but bounded parameters and the input vector.

A method according to another aspect of the invention is determining ifthe measurement is time-late by testing the sign of the update interval.If the measurement is time-late, apply the measurement to anout-of-sequence estimating filter that explicitly uses a mean squareoptimization criterion that separately accounts for measurement errorsand said bounding values, as well as the delay time, to produceestimates of the true state of the system. If the measurement is nottime-late, apply the measurement to an in-sequence estimating filterthat explicitly uses a mean square optimization criterion thatseparately accounts for measurement errors and said bounding values, toproduce estimates of the true state of the system. The said estimatesare applied to one of (a) make a decision relating to said system, (b)operate a control system, and (c) control a process.

An aspect of the invention relates to estimating the state of a systemhaving multidimensional parameters λ in addition to state variables x(k)at time t_(k) for k=1,2,3, . . . , which parameters λ are unknown,arbitrarily time-varying, but bounded, and driven by the nonlinear inputfunction u(x(k),λ) and governed by the state equationx(k+1)=Φx(k)+Γu(x(k),λ)  (1)where Φ,Γ are system matrices dependent on the discrete time intervalT=t_(k+1)−t_(k).

Another aspect of the invention relates to measuring aspects of thestate of the system governed by the measurement equationz(k)=Hx(k)+n(k)  (2)where n(k) is the measurement noise with covariance N and measurementmatrix H at time t_(k) for k=1,2,3, . . . .

The method comprises the steps of:

-   (a) Measuring aspects of the state of the system to produce initial    measurements;-   (b) Initializing a filter with an initial state estimate {circumflex    over (x)}(k₀|k₀) and matrices M(k₀|k₀),D(k₀|k₀) associated with the    initial state covariance and using a priori information and the    initial measurements. In general, a vector {circumflex over    (x)}(t|k) is defined as the estimate of the state of the system at    time t after processing k measurements z(i) for 1≦i≦k. The vector    {circumflex over (x)}(t_(j)|k) is denoted as {circumflex over    (x)}(j|k) when the time t=t_(j) is the time of the j^(th)    measurement for j=1,2,3, . . . . A matrix M(t|k) is defined as the    covariance of the state estimation errors at time t due only to the    errors in the measurements z(i) for 1≦i≦k and a priori initial    information that is independent of the parameter uncertainty. The    matrix M(t_(j)|k) is denoted as M(j|k), when the time t=t_(j) is the    time of the j^(th) measurement for j=1,2,3, . . . . A matrix D(t|k)    is defined as the matrix of bias coefficients, which linearly    relates state estimation errors to the parameter errors, at time t    (after processing k=0,1,2, . . . measurements). The matrix    D(t_(j)|k) is denoted as D(j|k), when the time t=t_(j) is the time    of the j^(th) measurement for j=1,2,3, . . . . The use of the    matrices M(j|k), and D(j|k) instead of S(j|k) is a difference of the    current invention from the prior art.-   (c) Determining the time t_(k+1) of a new measurement and the time t    when the filter was last updated. Using the update interval    T=t_(k+1)−t, determine the system transition matrices Φ and Γ. Note    that for the update interval T=t_(k+1)−t, the time t denotes the    time when the filter was last updated, whereas t_(k+1) is the time    of the new measurement, which can be prior to t due to delay. For    this case T≧0. If there is no delay, then usually t=t_(k), the time    of the previous measurement. For this case T≧0. Also determine the    mean value {overscore (λ)} of unknown but bounded parameters λ, and    the input vector u({circumflex over (x)}(t|k),{overscore (λ)}).-   (d) Measuring aspects z(k) of the state of the system where n(k) is    the measurement noise with covariance N and measurement matrix H at    time t_(k) for k=1,2,3, . . . ;-   (e) Determining whether the measurement is time-late by testing if    T<0. If T<0 then    -   1. Determining F,G as follows $\begin{matrix}        {{F = {\Phi + {\Gamma\;\frac{\partial u}{\partial x}}}}}_{{x = {\hat{x}{({t❘k})}}},{\lambda = \overset{\_}{\lambda}}} & (3)        \end{matrix}$ $\begin{matrix}        {{G = {\Gamma\;\frac{\partial u}{\partial\lambda}}}}_{{x = {\hat{x}{({t❘k})}}},{\lambda = \overset{\_}{\lambda}}} & (4)        \end{matrix}$    -   2. Generating a parameter matrix Λ representing physical bounds        on those parameters that are not state variables of the system.        This is a departure from the prior art, which uses a white        process noise covariance, W, that does not easily lend itself to        processing out-of-sequence measurements arriving later than an        update interval.    -   3. Extrapolate the state estimates {circumflex over (x)}(t|k)        and matrices M(t|k), D(t|k), to {circumflex over (x)}(k+1|k),        M(k+1|k), D(k+1|k),        {circumflex over (x)}(k+1|k)=Φ{circumflex over        (x)}(t|k)+Γu({circumflex over (x)}(t|k),{overscore (λ)})  (5)        M(k+1|k)=FM(t|k)F′  (6)        D(k+1|k)=FD(t|k)+G  (7)    -    and calculating S(k+1|k) as follows        S(k+1|k)=M(k+1|k)+D(k+1|k)ΛD(k+1|k)  (8)    -   4. Determine covariance of the residual Q as follows        Q=HS(k+1|k)H′+N  (9)    -   5. Determine the filter gain matrix K as follows        K=[M(t|k)F′+D(t|k)ΛD(k+1|k)′]H′Q ⁻¹  (10)    -   6. Determine the matrix L as follows        L=I−KHF  (11)    -    where I is the identity matrix;    -   7. Update the state estimate {circumflex over (x)}(k+1|k) as        {circumflex over (x)}(t|k+1)={circumflex over        (x)}(t|k)+K[z(k+1)−H{circumflex over (x)}(k+1|k)]  (12)    -   8. and the matrices M(k+1|k) and D(k+1|k) as        M(t|k+1)=LM(t|k)L′+KNK′  (13)        D(t|k+1)=D(t|k)−KHD(k+1|k)  (14)    -    and    -   9. Generating the total mean square error S(t|k+1) as follows        S(t|k+1)=M(t|k+1)+D(t|k+1)ΛD(t|k+1)′  (15)-   (f) Determining whether the measurement is time-late by testing if    T<0. If T≧0 then    -   1. Determine F,G using $\begin{matrix}        {{F = {\Phi + {\Gamma\;\frac{\partial u}{\partial x}}}}}_{{x = {\hat{x}{({k❘k})}}},{\lambda = \overset{\_}{\lambda}}} & (16)        \end{matrix}$ $\begin{matrix}        {{G = {\Gamma\;\frac{\partial u}{\partial\lambda}}}}_{{x = {\hat{x}{({k❘k})}}},{\lambda = \overset{\_}{\lambda}}} & (17)        \end{matrix}$    -   2. Generate a parameter matrix A, representing physical bounds        on those parameters that are not state variables of the system;    -   3. Extrapolate said state estimates {circumflex over (x)}(k|k)        and matrices M(k|k), D(k|k), S(k|k) to {circumflex over        (x)}(k+1|k), M(k+1|k), D(k+1|k), and S(k+1|k) as follows        {circumflex over (x)}(k+1|k)=Φ{circumflex over        (x)}(k|k)+Γu({circumflex over (x)}(k|k),{overscore (λ)})  (18)        M(k+1|k)=FM(k|k)F′  (19)        D(k+1|k)=FD(k|k)+G  (20)        S(k+1|k)=M(k+1|k)+D(k+1|k)ΛD(k+1|k)′  (21)    -   4. Determine covariance of the residual Q as follows        Q=HS(k+1|k)H′+N  (22)    -   5. Determine the filter-gain matrix K as follows        K=S(k+1|k)H′Q ⁻¹  (23)    -   6. Determine the matrix L as follows        L=I−KH  (24)    -    where I is the identity matrix;    -   7. Update the state estimate {circumflex over (x)}(k+1|k) as        follows        {circumflex over (x)}(k+1|k+1)={circumflex over        (x)}(k+1|k)+K[z(k+1)−H{circumflex over (x)}(k+1|k)]  (25)    -   8. Update the matrices M(k+1|k) and D(k+1|k) as        M(k+1|k+1)=LM(k+1|k)L′+KNK′  (26)        D(k+1|k+1)=LD(k+1|k)  (27)    -    and    -   9. Generate the total mean square error S(k+1|k+1) as follows        S(k+1|k+1)=M(k+1|k+1)+D(k+1|k+1)ΛD(k+1|k+1)′  (28)

BRIEF DESCRIPTION OF THE DRAWING

FIG. 1 is a simplified block diagram of a system for estimating thestate of a target, which in this case is the track of an aircraft, afterprocessing measurements with time delays from multiple sensors ofsystems; and

FIGS. 2 and 3 together constitute a simplified logic flow chart ordiagram illustrating the recursive processing associated with areduced-state estimator according to an aspect of the invention, whichmay be used in the arrangement of FIG. 1.

DESCRIPTION OF THE INVENTION

The solution of the problem of state estimation after processingmeasurements with time delays from multiple sensors of systemscharacterized by state variables and by multidimensional parameters, forwhich the latter are unknown and may vary arbitrarily in time withinknown physical bounds requires a completely different method, which isincorporated in an aspect of the invention. The simplified logic flowchart or diagram 200 of FIG. 2 illustrates a method according to anaspect of the invention. In FIG. 2, the logic flow of the inventionbegins in a block 210 with initialization of the state estimate{circumflex over (x)}(k₀|k₀) and the matrices M(k₀|k₀), D(k₀|k₀). Ingeneral, a vector {circumflex over (x)}(t|k) is defined as the estimateof the state of the system at time t after processing k measurementsz(i) for 1≦i≦k. The vector {circumflex over (x)}(t_(j)|k) is denoted as{circumflex over (x)}(j|k) when the time t=t_(j) is the time of thej^(th) measurement for j=1,2,3, . . . . A matrix M(t|k) is defined asthe covariance of the state estimation errors at time t due only to theerrors in the measurements z(i) for 1≦i≦k and a priori initialinformation that is independent of the parameter uncertainty. The matrixM(t_(j)|k) is denoted as M(j|k), when the time t=t_(j) is the time ofthe j^(th) measurement for j=1,2,3, . . . . A matrix D(t|k) is definedas the matrix of bias coefficients, which linearly relates stateestimation errors to the parameter errors, at time t (after processingk=0,1,2, . . . measurements). The matrix D(t_(j)|k) is denoted asD(j|k), when the time t=t_(j) is the time of the j^(th) measurement forj=1,2,3, . . . . The use of the matrices M(j|k), and D(j|k) instead ofS(j|k) is a difference of the current invention from the prior art. Fromblock 210, the logic of the invention of FIG. 2 flows to a block 212,which represents the incrementing of a time index k to k+1.

The logic of the invention then flows to a further block 214, whichrepresents calculating the update interval according toT=t _(k+1) −t  (29)and determining the system transition matrices Φ,Γ, and the mean value{overscore (λ)} of unknown but bounded parameters λ and also the inputvector u({circumflex over (x)}(t|k),{overscore (λ)}). Note that for theupdate interval T=t_(k+1)−t, the time t denotes the time when the filterwas last updated, whereas t_(k+1) is the time of the new measurement,which can be prior to t due to delay. For this case T<0. If there is nodelay, then usually t=t_(k), the time of the previous measurement. Forthis case T≧0.

From block 214 of FIG. 2, the logic flows to a block 216, whichrepresents the measurement z(k+1) of aspects of the state of the system.This step is same as the corresponding step of the prior art.

In decision block 218 the latency of the measurement is determined bytesting the sign of the update interval, i.e., T<0.

If T<0, the logic of the invention then flows from decision block 218 byway of the YES output and a path 219 to a block 220, in which thematrices F and G are calculated according to $\begin{matrix}{{F = {\Phi + {\Gamma\;\frac{\partial u}{\partial x}}}}}_{{x = {\hat{x}{({t❘k})}}},{\lambda = \overset{\_}{\lambda}}} & (30)\end{matrix}$ $\begin{matrix}{{G = {\Gamma\;\frac{\partial u}{\partial\lambda}}}}_{{x = {\hat{x}{({t❘k})}}},{\lambda = \overset{\_}{\lambda}}} & (31)\end{matrix}$

From block 220, the logic of the invention of FIG. 2 flows to a block222, which accesses or inputs a parameter matrix Λ. The parameter matrixΛ represents the physical bounds on the unknown parameters, such as turnrate and tangential acceleration, and is preferably selected based on aprecise description of the physical system, and depends upon well-knownphysical phenomena and characteristics of real systems. Types ofphysical parameters, which might be considered for determination of Λ inthe case of maneuvers, are the turn rate ω and the tangentialacceleration α. This is a departure from the prior art, which uses awhite process noise covariance W that does not correctly model the classof systems addressed by the invention herein.

From block 222 of FIG. 2, the logic flow of the invention proceeds to ablock 224, which represents the extrapolation of state estimates{circumflex over (x)}(t|k) and matrices M(t|k), D(t|k), according to{circumflex over (x)}(k+1|k)=Φ{circumflex over (x)}(t|k)+Γu({circumflexover (x)}(t|k),{overscore (λ)})  (32)M(k+1|k)=FM(t|k)F′  (33)D(k+1|k)=FD(t|k)+G  (34)and the calculation of the state covariance S(k+1|k) according toS(k+1|k)=M(k+1|k)+D(k+1|k)ΛD(k+1|k)′  (35)Thus, there are several differences between the invention herein asexemplified in FIG. 2 and the prior art. First, {overscore (λ)} in theinput function u({circumflex over (x)}(t|k),{overscore (λ)}) in formula(32) refers to the mean value of the parameters λ, whose bounds arerepresented by the matrix Λ in formula (35). Note that the inputfunction u(x(k),λ) may be a nonlinear function of the state variablesx(k) and the multidimensional parameters λ. Another difference is thatthe prior art Kalman filter extrapolates the state covariance, while thecurrent invention extrapolates the matrices M(t|k) and D(t|k) toM(k+1|k) and D(k+1|k), respectively, as in formulas (33) and (34), andthen forms the total extrapolated covariance S(k+1|k) from M(k+1|k),D(k+1|k), and Λ as in formula (35).

From block 224 of FIG. 2, the logic of the invention flows to a block226, which represents the accessing or inputting of the measurementnoise covariance N. Block 228 of FIG. 2 computes filter matrix Q, filtergain matrix K, and filter matrix L. While generically similar to theprior art, the filter gain matrix K obtained in block 228 of thearrangement of FIG. 2 are different from the filter gain matrix Kobtained in prior art because of the use of M(k+1|k), D(k+1|k) and Λ toconstruct S(k+1|k) in equation (35). The covariance of the residual Q iscalculated in block 228 asQ=HS(k+1|k)H′+N  (36)The filter gain matrix K is calculated asK=[M(t|k)F′+D(t|k)ΛD(k+1|k)′]H′Q ⁻¹  (37)and the matrix L is calculated asL=I−KHF  (38)where I is the identity matrix.

The logic flows from block 228 of FIG. 2 to a block 230, whichrepresents the updating of the state estimate and the associatedmatrices. More particularly, the state estimate {circumflex over(x)}(t|k+1) is calculated as{circumflex over (x)}(t|k+1)={circumflex over(x)}(t|k)+K[z(k+1)−H{circumflex over (x)}(k+1|k)]  (39)The matrices M(t|k+1) and D(t|k+1) are calculated asM(t|k+1)=LM(t|k)L′+KNK′  (40)andD(t|k+1)=D(t|k)−KHD(k+1k)  (41)respectively. Finally, the matrix of the total covariance S(t|k+1) iscalculated asS(t|k+1)=M(t|k+1)+D(t|k+1)ΛD(t|k+1)′  (42)and equation (42) represents a mean-square criterion which may be usedfor a root-mean-square determination. The logic of FIG. 2 returns fromblock 230 to block 212 by way of a path 231.

Returning now to decision block 218 of FIG. 2, the logic may leave block218 by the NO output if T≧0, and in that case flows by way of a path 239to block 232 of FIG. 3. Block 232 of FIG. 3 represents calculation ofthe matrices F and G according to $\begin{matrix}{{F = {\Phi + {\Gamma\;\frac{\partial u}{\partial x}}}}}_{{x = {\hat{x}{({k❘k})}}},{\lambda = \overset{\_}{\lambda}}} & (43)\end{matrix}$ $\begin{matrix}{{G = {\Gamma\;\frac{\partial u}{\partial\lambda}}}}_{{x = {\hat{x}{({k❘k})}}},{\lambda = \overset{\_}{\lambda}}} & (44)\end{matrix}$

From block 232 of FIG. 3, the logic of the invention of FIG. 3 flows toa block 234, which accesses or inputs a parameter matrix Λ. Theparameter matrix Λ represents the physical bounds on the unknownparameters, such as turn rate and tangential acceleration, and ispreferably selected based on a description of the physical system, whichshould be as precise as possible or practicable. The parameter matrix Λdepends upon well-known physical phenomena and characteristics of realsystems. Types of physical parameters, which might be considered fordetermination of Λ in the case of maneuvers, are the turn rate ω and thetangential acceleration α. This is a departure from the prior art, whichuses a white process noise covariance W that does not correctly modelthe class of systems addressed by the invention herein.

From block 234 of FIG. 3, the logic flow of the invention proceeds to ablock 236, which represents the extrapolation of state estimates{circumflex over (x)}(k|k), and matrices M(k|k), D(k|k), according to{circumflex over (x)}(k+1|k)=Φ{circumflex over (x)}(k|k)+Γu({circumflexover (x)}(k|k),{overscore (λ)})  (45)M(k+1|k)=FM(k|k)F′  (46)D(k+1|k)=FD(k|k)+G  (47)and the calculation of the state covariance S(k+1|k) according toS(k+1|k)=M(k+1|k)+D(k+1|k)ΛD(k+1|k)′  (48)Thus, another difference between the invention herein and the prior artis that the prior art Kalman filter extrapolates state estimate{circumflex over (x)}(k|k) and state covariance S(k|k) to {circumflexover (x)}(k+1|k) and S(k+1|k), while the current invention extrapolatesstate estimate {circumflex over (x)}(k|k) and matrices M(k|k),D(k|k) to{circumflex over (x)}(k+1|k) and M(k+1|k),D(k+1|k) respectively.

From block 236 of FIG. 3, the logic of the invention flows to a block238, which represents the accessing or inputting of the measurementnoise covariance N. Block 240 of FIG. 2 computes filter matrix Q, filtergain matrix K, and filter matrix L. While generically similar to theprior art, the filter gain matrix K obtained in block 240 of thearrangement of FIG. 3 are different from the filter gain matrix Kobtained in prior art because of the use of M(k+1|k),D(k+1|k) and Λ toformula S(k+1|k) in equation (48). The covariance of the residual Q iscalculated asQ=HS(k+1|k)H′+N  (49)The filter gain matrix K is calculated asK=S(k+1|k)H′Q ⁻¹  (50)and the matrix L is calculated asL=I−KH  (51)where I is the identity matrix.

The logic flows from block 240 of FIG. 3 to a block 242, whichrepresents the updating of the state estimate and the associatedmatrices. More particularly, the state estimate {circumflex over(x)}(k+1|k+1) is calculated as{circumflex over (x)}(k+1|k+1)={circumflex over(x)}(k+1|k)+K[z(k+1)−H{circumflex over (x)}(k+1|k)]  (52)The matrices M(k+1|k) and D(k+1|k) are calculated asM(k+1|k+1)=LM(k+1|k)L′+KNK′  (53)andD(k+1|k+1)=LD(k+1|k)  (54)respectively. Finally, the matrix of the total covariance S(k+1|k+1) iscalculated asS(k+1|k+1)=M(k+1|k+1)+D(k+1|k+1)ΛD(k+1|k+1)′  (55)The logic leaves block 242 of FIG. 3 and returns to block 212 of FIG. 2by way of a logic path 243. The above equations (29)–(55) are set forthin FIGS. 2 and 3, and in P. Mookerjee and F. Reifler, “Reduced StateEstimator for Systems with Parametric Inputs,” IEEE Transactions onAerospace and Electronic Systems, pp. 446–461, Vol. AES-40, No. 2, Apr.2004.

The calculations associated with block 236 of FIG. 3 separate the stateestimation error covariance S(j|k) into components, M(j|k) andD(j|k)ΛD(j|k)′, attributable to measurement error and parameteruncertainty, and separately propagates these covariances from one timeindex k to next time index k+1. In addition, the gain matrix K iscomputed based on these propagated covariances. The gains weight themeasurements to form the state estimates. The values of these gains socomputed are, of course, different from those computed by the prior artmethod.

A salient difference between the prior-art method and that of theinvention is the introduction into the equations defining amultidimensional state estimation error covariance denoted above asM(j|k), attributable to measurement error and D(j|k)ΛD(j|k)′,representing the physical bounds of the parameters, and propagatingcertain coefficients (denoted as D(k|k) and D(k+1|k)). The matrix M(j|k)is defined as the covariance of the state estimation errors at timet_(j) due only to the errors in the measurements z(i) for 1≦i≦k and apriori initial information that is independent of the parameteruncertainty. D(j|k) is defined as the matrix of bias coefficients, whichlinearly relates state estimation errors to the parameter errors, attime t_(j) (after processing k=0,1,2, . . . measurements). The articleby Portmann, Moore, and Bath 1990 mentions, “The optimum approach is toreorder the measurements in time and reprocess them in time order. Ifthe measurements are badly scrambled, however, this can significantlyincrease the computational burden of the filter and may not be an optionopen to the filter designer. The SCF filter can be modified to produce asuboptimal estimate for a measurement out of time sequence whichrequires little more computation than for a measurement in timesequence. The criteria for optimality is somewhat different from thatdescribed in the preceding sections, but the form of the filter is verynearly the same.” No algorithm is provided in their article supportingthe abovementioned statements.

Thus, the invention uses a novel mean-square optimization criterion(equation (42) or (48)) which explicitly addresses the known physicalbounds of the multidimensional parameters, and incorporates analyticalmodeling of the parameter bounds, whose modeling may be as precise asknowledge of the boundary values permits. The invention provides anexact implementable recursive solution that optimizes the mean-squarecriterion.

1. A method for recursively estimating the state of a system havingmultidimensional parameters λ in addition to state variables x(k) attime t_(k) for k=1,2,3, . . . , which parameters λ are unknown,arbitrarily time-varying, but bounded, and driven by the input functionu(x(k),λ) and governed by the state equationx(k+1)=Φx(k)+Γu(x(k),λ) where Φ,Γ are system matrices dependent on thediscrete time interval T=t_(k+1)−t_(k): said method comprising thefollowing steps: measuring aspects of the state of the system to produceinitial measurements; initializing state estimates {circumflex over(x)}(k₀|k₀) and the matrices M(k₀|k₀), D(k₀|k₀) using a prioriinformation and the initial measurements, where vector {circumflex over(x)}(t|k) is defined as the estimate of the state of the system at timet after processing the measurements z(i) for 1≦i≦k; vector {circumflexover (x)}(t_(j)|k) is denoted as {circumflex over (x)}(j|k) when thetime t=t_(j) is the time of the j^(th) measurement for j=1,2,3, . . . ;matrix M(t|k) is defined as the covariance of the state estimationerrors at time t due only to the errors in the measurements z(i) for1≦i≦k and a priori initial information that is independent of theparameter uncertainty; matrix M(t_(j)|k) is denoted as M(j|k), when thetime t=t_(j) is the time of the j^(th) measurement for j=1,2,3, . . . ;matrix D(t|k) is defined as the matrix of bias coefficients, whichlinearly relates state estimation errors to the parameter errors, attime t (after processing k=0,1,2, . . . measurements); matrix D(t_(j)|k)is denoted as D(j|k), when the time t=t_(j) is the time of the j^(th)measurement for j=1,2,3, . . . ; determining the time t_(k+1) of a newmeasurement and the time t when the filter was last updated; using theupdate interval T=t_(k+1)−t, determine the system transition matrices Φand Γ; determining the mean value {overscore (λ)} of unknown but boundedparameters λ, and the input vector u({circumflex over(x)}(t|k),{overscore (λ)}); measuring aspects of the state of the systemgoverned by the measurement equationz(k)=Hx(k)+n(k)  where n(k) is the measurement noise with covariance Nand measurement matrix H at time t_(k) for k=1,2,3, . . . ; determiningif the measurement is time-late by testing T<0; (a) if the measurementis time-late determining F,G as follows${{F = {\Phi + {\Gamma\;\frac{\partial u}{\partial x}}}}}_{{x = {\hat{x}{({t❘k})}}},{\lambda = \overset{\_}{\lambda}}}$${{G = {\Gamma\;\frac{\partial u}{\partial\lambda}}}}_{{x = {\hat{x}{({t❘k})}}},{\lambda = \overset{\_}{\lambda}}}$generating a parameter matrix Λ, representing physical bounds on theparameters λ that are not state variables of the system; extrapolatingsaid state estimates {circumflex over (x)}(t|k) and matrices M(t|k),D(t|k), to {circumflex over (x)}(k+1|k), M(k+1|k), D(k+1|k), andcalculating S(k+1|k) as in{circumflex over (x)}(k+1|k)=Φ{circumflex over (x)}(t|k)+Γu({circumflexover (x)}(t|k),{overscore (λ)})M(k+1|k)=FM(t|k)F′D(k+1|k)=FD(t|k)+GS(k+1|k)=M(k+1|k)+D(k+1|k)ΛD(k+1|k)′ determining covariance of theresidual Q as inQ=HS(k+1|k)H′+N determining the filter gain matrix K as inK=[M(t|k)F′+D(t|k)ΛD(k+1|k)′]H′Q ⁻¹ determining the matrix L as inL=I−KHF  where I is the identity matrix; updating the state estimate{circumflex over (x)}(t|k) as{circumflex over (x)}(t|k+1)={circumflex over(x)}(t|k)+K[z(k+1)−H{circumflex over (x)}(k+1|k)] updating the matricesM(t|k) and D(t|k) asM(t|k+1)=LM(t|k)L′+KNK′D(t|k+1)=D(t|k)−KHD(k+1|k)  respectively, and generating the total meansquare error S(t|k+1) as inS(t|k+1)=M(t|k+1)+D(t|k+1)ΛD(t|k+1)′ (b) and if the measurement is nottime-late determining F,G using${{F = {\Phi + {\Gamma\;\frac{\partial u}{\partial x}}}}}_{{x = {\hat{x}{({k❘k})}}},{\lambda = \overset{\_}{\lambda}}}$and,${{G = {\Gamma\;\frac{\partial u}{\partial\lambda}}}}_{{x = {\hat{x}{({k❘k})}}},{\lambda = \overset{\_}{\lambda}}}$generating a parameter matrix Λ, representing physical bounds on thoseparameters that are not state variables of the system; extrapolatingsaid state estimates {circumflex over (x)}(k|k) and matrices M(k|k),D(k|k), S(k|k) to {circumflex over (x)}(k+1|k), M(k+1|k), D(k+1|k), andS(k+1|k) as in{circumflex over (x)}(k+1|k)=Φ{circumflex over (x)}(k|k)+Γu({circumflexover (x)}(k|k),{overscore (λ)})M(k+1|k)=FM(k|k)F′D(k+1|k)=FD(k|k)+GS(k+1|k)=M(k+1|k)+D(k+1|k)ΛD(k+1|k)′ determining covariance of theresidual Q as inQ=HS(k+1|k)H′+N determining the filter gain matrix K as inK=S(k+1|k)H′Q ⁻¹ determining the matrix L as inL=I−KH  where I is the identity matrix; updating the state estimate{circumflex over (x)}(k+1|k) as{circumflex over (x)}(k+1|k+1)={circumflex over(x)}(k+1|k)+K[z(k+1)−H{circumflex over (x)}(k+1|k)] updating thematrices M(k+1|k) and D(k+1|k) asM(k+1|k+1)=LM(k+1|k)L′+KNK′ and,D(k+1|k+1)=LD(k+1|k) respectively, and generating the total mean squareerror S(k+1|k+1) asS(k+1|k+1)=M(k+1|k+1)+D(k+1|k+1)ΛD(k+1|k+1)′ after
 2. A method forestimating the state of a system comprising the steps of: observing asystem having state variables and also having unknown, multidimensional,arbitrarily time-varying parameters, but which are subject to knownbounded values and driven by a time-varying input function that dependson the states, and multidimensional parameters; measuring certainaspects of the state of the system in the presence of measurement errorsto produce initial measurements; initializing state estimates andmatrices using a priori information and the initial measurements; usingthe update interval in determining the system transition matrices andthe mean value of unknown but bounded parameters and the input vector;determining if the measurement is time-late by testing the sign of theupdate interval; if the measurement is time-late applying themeasurements to an out-of-sequence estimating filter that explicitlyuses a mean square optimization criterion that separately accounts formeasurement errors and said bounding values, as well as the delay time,to produce estimates of the true state of the system; if the measurementis not time-late applying the measurements to an in-sequence estimatingfilter that explicitly uses a mean square optimization criterion thatseparately accounts for measurement errors and said bounding values, toproduce estimates of the true state of the system; applying saidestimates to one of (a) make a decision relating to said system, (b)operate a control system, and (c) control a process.